import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from mpl_toolkits.mplot3d import Axes3D
import time
import logging
from dataclasses import dataclass
from typing import List, Tuple, Callable, Optional, Dict
import copy
import math

# 配置中文字体显示
plt.rcParams["font.sans-serif"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"]
plt.rcParams["axes.unicode_minus"] = False

# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)

@dataclass
class Parameters:
    """算法参数配置类"""
    # PSO算法参数
    num_particles: int = 80  # 粒子数量
    max_iterations: int = 200  # 最大迭代次数
    w_max: float = 0.9  # 最大惯性权重
    w_min: float = 0.4  # 最小惯性权重
    c1: float = 2.0  # 个体学习因子
    c2: float = 2.0  # 全局学习因子
    perturbation_interval: int = 20  # 精英扰动间隔代数
    perturbation_strength: float = 0.05  # 扰动强度
    penalty_factor: float = 100.0  # 罚函数因子
    
    # 物理参数
    g: float = 9.8  # 重力加速度(m/s²)
    v_m: float = 300.0  # 导弹速度(m/s)
    smoke_radius: float = 10.0  # 烟幕半径(m)
    smoke_duration: float = 20.0  # 烟幕持续时间(s)
    smoke_sink_speed: float = 3.0  # 烟幕下沉速度(m/s)
    view_angle: float = np.pi / 6  # 导弹视场半顶角(30度)
    
    # 初始位置参数
    uav_initial_pos: Tuple[float, float, float] = (17800, 0, 1800)  # 无人机初始位置
    missile_initial_pos: Tuple[float, float, float] = (20000, 0, 2000)  # 导弹初始位置
    
    # 真目标参数
    target_radius: float = 7.0  # 真目标半径(m)
    target_height: float = 10.0  # 真目标高度(m)
    target_center: Tuple[float, float, float] = (0, 200, 5)  # 真目标中心位置
    
    # 约束条件
    min_uav_speed: float = 70.0  # 无人机最小速度(m/s)
    max_uav_speed: float = 140.0  # 无人机最大速度(m/s)
    min_theta: float = 0.0  # 方位角最小值(rad)
    max_theta: float = 2 * np.pi  # 方位角最大值(rad)
    min_phi: float = -np.pi / 6  # 仰角最小值(rad)
    max_phi: float = np.pi / 6  # 仰角最大值(rad)
    min_delay: float = 0.5  # 最小起爆延迟(s)
    min_zq: float = 60.0  # 最小起爆点高度(m)
    max_drop_time: float = 46.58  # 最大投放时间(s)
    
    # 计算参数
    time_step: float = 0.01  # 时间步长(s)
    convergence_threshold: float = 0.001  # 收敛阈值
    convergence_window: int = 20  # 收敛判断窗口(代数)

class SmokeOptimizer:
    """烟幕干扰弹最优投放策略优化器"""
    
    def __init__(self, params: Optional[Parameters] = None):
        """初始化优化器
        
        Args:
            params: 参数配置对象，若为None则使用默认参数
        """
        self.params = params if params is not None else Parameters()
        
        # 预计算真目标关键点位
        self.target_key_points = self._generate_target_key_points()
        
        # 预计算导弹击中目标时间
        self.missile_hit_time = self._calculate_missile_hit_time()
        
        # 预计算导弹视场有效时间区间
        self.effective_view_time = self._calculate_effective_view_time()
        
        # 算法结果
        self.best_solution = None
        self.best_fitness = -np.inf
        self.fitness_history = []
        self.global_best_history = []
        
    def _generate_target_key_points(self) -> List[Tuple[float, float, float]]:
        """生成真目标的8个关键点位
        
        Returns:
            包含8个关键点位坐标的列表
        """
        cx, cy, cz = self.params.target_center
        r = self.params.target_radius
        h = self.params.target_height
        
        # 下底面4个点
        points = [
            (cx + r, cy, cz),
            (cx - r, cy, cz),
            (cx, cy + r, cz),
            (cx, cy - r, cz),
            # 上底面4个点
            (cx + r, cy, cz + h),
            (cx - r, cy, cz + h),
            (cx, cy + r, cz + h),
            (cx, cy - r, cz + h)
        ]
        return points
    
    def _calculate_missile_hit_time(self) -> float:
        """计算导弹击中假目标的时间
        
        Returns:
            导弹击中假目标的时间(s)
        """
        mx0, my0, mz0 = self.params.missile_initial_pos
        distance = np.sqrt(mx0**2 + my0**2 + mz0**2)
        return distance / self.params.v_m
    
    def _calculate_effective_view_time(self) -> Tuple[float, float]:
        """计算导弹视场有效时间区间
        
        Returns:
            包含开始时间和结束时间的元组
        """
        # 简化计算，通过二分法找到真目标进入和离开视场的时间
        t_start = 0.0
        t_end = self.missile_hit_time
        
        # 找到进入视场的时间
        t_in = self._find_view_boundary(t_start, t_end, True)
        
        # 找到离开视场的时间
        t_out = self._find_view_boundary(t_start, t_end, False)
        
        return (t_in, t_out)
    
    def _find_view_boundary(self, t_start: float, t_end: float, find_in: bool) -> float:
        """使用二分法找到真目标进入或离开视场的边界时间
        
        Args:
            t_start: 搜索起始时间
            t_end: 搜索结束时间
            find_in: 若为True则寻找进入视场的时间，否则寻找离开视场的时间
            
        Returns:
            边界时间
        """
        eps = 0.01
        while t_end - t_start > eps:
            t_mid = (t_start + t_end) / 2
            in_view = self._is_target_in_view(t_mid)
            
            if find_in:
                if in_view:
                    t_end = t_mid
                else:
                    t_start = t_mid
            else:
                if in_view:
                    t_start = t_mid
                else:
                    t_end = t_mid
                    
        return (t_start + t_end) / 2
    
    def _is_target_in_view(self, t: float) -> bool:
        """判断真目标是否在导弹视场内
        
        Args:
            t: 时间
            
        Returns:
            若真目标在视场内则返回True，否则返回False
        """
        missile_pos = self._get_missile_position(t)
        missile_dir = self._get_missile_direction()
        
        # 检查所有关键点位是否至少有一个在视场内
        for point in self.target_key_points:
            # 计算目标点到导弹的向量
            target_vec = np.array(point) - np.array(missile_pos)
            target_dist = np.linalg.norm(target_vec)
            
            if target_dist < 1e-6:
                return True  # 导弹已到达目标位置
            
            # 计算向量与导弹方向的夹角余弦
            cos_angle = np.dot(target_vec, missile_dir) / target_dist
            
            # 如果夹角小于视场角，则在视场内
            if cos_angle >= np.cos(self.params.view_angle):
                return True
                
        return False
    
    def _get_missile_direction(self) -> np.ndarray:
        """获取导弹飞行方向单位向量
        
        Returns:
            导弹飞行方向单位向量
        """
        mx0, my0, mz0 = self.params.missile_initial_pos
        dir_vec = np.array([-mx0, -my0, -mz0])
        return dir_vec / np.linalg.norm(dir_vec)
    
    def _get_missile_position(self, t: float) -> Tuple[float, float, float]:
        """计算t时刻导弹的位置
        
        Args:
            t: 时间
            
        Returns:
            导弹位置坐标(x, y, z)
        """
        mx0, my0, mz0 = self.params.missile_initial_pos
        dir_vec = self._get_missile_direction()
        
        # 计算导弹在t时刻的位置
        distance_traveled = self.params.v_m * t
        x = mx0 + dir_vec[0] * distance_traveled
        y = my0 + dir_vec[1] * distance_traveled
        z = mz0 + dir_vec[2] * distance_traveled
        
        return (x, y, z)
    
    def _get_uav_position(self, t: float, theta: float, phi: float, v_u: float) -> Tuple[float, float, float]:
        """计算t时刻无人机的位置
        
        Args:
            t: 时间
            theta: 方位角
            phi: 仰角
            v_u: 无人机速度
            
        Returns:
            无人机位置坐标(x, y, z)
        """
        x0, y0, z0 = self.params.uav_initial_pos
        
        # 计算方向向量
        dir_x = np.cos(theta) * np.cos(phi)
        dir_y = np.sin(theta) * np.cos(phi)
        dir_z = np.sin(phi)
        
        # 计算位置
        x = x0 + v_u * t * dir_x
        y = y0 + v_u * t * dir_y
        z = z0 + v_u * t * dir_z
        
        return (x, y, z)
    
    def _get_smoke_release_position(self, theta: float, phi: float, v_u: float, t_t: float) -> Tuple[float, float, float]:
        """计算烟幕弹投放点位置
        
        Args:
            theta: 方位角
            phi: 仰角
            v_u: 无人机速度
            t_t: 投放时间
            
        Returns:
            投放点位置坐标(x, y, z)
        """
        return self._get_uav_position(t_t, theta, phi, v_u)
    
    def _get_smoke_detonate_position(self, theta: float, phi: float, v_u: float, t_t: float, delta_tq: float) -> Tuple[float, float, float]:
        """计算烟幕弹起爆点位置
        
        Args:
            theta: 方位角
            phi: 仰角
            v_u: 无人机速度
            t_t: 投放时间
            delta_tq: 起爆延迟时间
            
        Returns:
            起爆点位置坐标(x, y, z)
        """
        # 投放点位置
        sx, sy, sz = self._get_smoke_release_position(theta, phi, v_u, t_t)
        
        # 计算方向向量
        dir_x = np.cos(theta) * np.cos(phi)
        dir_y = np.sin(theta) * np.cos(phi)
        dir_z = np.sin(phi)
        
        # 水平方向位移（惯性运动）
        x = sx + v_u * dir_x * delta_tq
        y = sy + v_u * dir_y * delta_tq
        
        # 竖直方向位移（受重力影响）
        z = sz + v_u * dir_z * delta_tq - 0.5 * self.params.g * delta_tq**2
        
        return (x, y, z)
    
    def _get_smoke_position(self, detonate_pos: Tuple[float, float, float], t: float, t_q: float) -> Tuple[float, float, float]:
        """计算t时刻烟幕云团中心位置
        
        Args:
            detonate_pos: 起爆点位置
            t: 当前时间
            t_q: 起爆时间
            
        Returns:
            烟幕云团中心位置坐标(x, y, z)
        """
        x, y, z = detonate_pos
        # 烟幕匀速下沉
        sink_distance = self.params.smoke_sink_speed * max(0, t - t_q)
        return (x, y, z - sink_distance)
    
    def _segment_sphere_intersection(self, a: Tuple[float, float, float], b: Tuple[float, float, float], 
                                    c: Tuple[float, float, float], r: float) -> bool:
        """判断线段AB是否与球体C相交
        
        Args:
            a: 线段端点A
            b: 线段端点B
            c: 球体中心C
            r: 球体半径
            
        Returns:
            若相交则返回True，否则返回False
        """
        a = np.array(a)
        b = np.array(b)
        c = np.array(c)
        
        # 线段向量
        ab = b - a
        # 线段长度平方
        ab_len_sq = np.dot(ab, ab)
        
        if ab_len_sq < 1e-12:  # A和B点重合
            return np.linalg.norm(a - c) <= r
        
        # 计算参数
        ac = a - c
        a_coeff = ab_len_sq
        b_coeff = 2 * np.dot(ac, ab)
        c_coeff = np.dot(ac, ac) - r**2
        
        # 求解二次方程 ax² + bx + c = 0
        discriminant = b_coeff**2 - 4 * a_coeff * c_coeff
        
        if discriminant < 0:
            return False  # 无交点
        
        sqrt_d = np.sqrt(discriminant)
        t1 = (-b_coeff - sqrt_d) / (2 * a_coeff)
        t2 = (-b_coeff + sqrt_d) / (2 * a_coeff)
        
        # 检查是否有解在[0, 1]区间内
        return (t1 <= 1 and t1 >= 0) or (t2 <= 1 and t2 >= 0)
    
    def _is_fully_obscured(self, t: float, smoke_pos: Tuple[float, float, float]) -> bool:
        """判断真目标是否完全被烟幕遮蔽
        
        Args:
            t: 时间
            smoke_pos: 烟幕云团中心位置
            
        Returns:
            若完全遮蔽则返回True，否则返回False
        """
        missile_pos = self._get_missile_position(t)
        
        # 检查所有关键点位的视线是否都被烟幕遮蔽
        for point in self.target_key_points:
            if not self._segment_sphere_intersection(
                missile_pos, point, smoke_pos, self.params.smoke_radius
            ):
                return False
        return True
    
    def _calculate_constraint_violations(self, theta: float, phi: float, v_u: float, 
                                        t_t: float, delta_tq: float) -> float:
        """计算约束违反量
        
        Args:
            theta: 方位角
            phi: 仰角
            v_u: 无人机速度
            t_t: 投放时间
            delta_tq: 起爆延迟时间
            
        Returns:
            约束总违反量
        """
        violations = 0.0
        
        # 速度约束
        if v_u < self.params.min_uav_speed:
            violations += self.params.min_uav_speed - v_u
        elif v_u > self.params.max_uav_speed:
            violations += v_u - self.params.max_uav_speed
            
        # 方位角约束
        if theta < self.params.min_theta:
            violations += self.params.min_theta - theta
        elif theta > self.params.max_theta:
            violations += theta - self.params.max_theta
            
        # 仰角约束
        if phi < self.params.min_phi:
            violations += self.params.min_phi - phi
        elif phi > self.params.max_phi:
            violations += phi - self.params.max_phi
            
        # 起爆延迟约束
        if delta_tq < self.params.min_delay:
            violations += self.params.min_delay - delta_tq
            
        # 投放时间约束
        if t_t > self.params.max_drop_time:
            violations += t_t - self.params.max_drop_time
            
        # 烟幕起爆点高度约束
        x_q, y_q, z_q = self._get_smoke_detonate_position(theta, phi, v_u, t_t, delta_tq)
        if z_q < self.params.min_zq:
            violations += self.params.min_zq - z_q
            
        return violations
    
    def _calculate_fitness(self, particle: np.ndarray) -> float:
        """计算粒子的适应度
        
        Args:
            particle: 粒子位置，包含(theta, phi, v_u, t_t, delta_tq)
            
        Returns:
            粒子适应度值
        """
        theta, phi, v_u, t_t, delta_tq = particle
        
        # 计算约束违反量
        constraint_violations = self._calculate_constraint_violations(theta, phi, v_u, t_t, delta_tq)
        
        # 如果有约束违反，应用罚函数
        if constraint_violations > 1e-6:
            return -self.params.penalty_factor * constraint_violations
        
        # 计算起爆时间
        t_q = t_t + delta_tq
        
        # 计算起爆点位置
        detonate_pos = self._get_smoke_detonate_position(theta, phi, v_u, t_t, delta_tq)
        
        # 确定有效时间区间
        t_in, t_out = self.effective_view_time
        t_start = max(t_q, t_in)
        t_end = min(t_q + self.params.smoke_duration, 
                   t_q + detonate_pos[2] / self.params.smoke_sink_speed,
                   t_out, self.missile_hit_time)
        
        # 如果时间区间无效
        if t_start >= t_end:
            return 0.0
        
        # 计算遮蔽时长
        obscured_duration = 0.0
        t = t_start
        
        while t < t_end:
            # 计算当前烟幕位置
            smoke_pos = self._get_smoke_position(detonate_pos, t, t_q)
            
            # 判断是否完全遮蔽
            if self._is_fully_obscured(t, smoke_pos):
                obscured_duration += self.params.time_step
                
            t += self.params.time_step
            
        return obscured_duration
    
    def _initialize_particles(self) -> Tuple[np.ndarray, np.ndarray]:
        """初始化粒子群
        
        Returns:
            粒子位置和速度的数组
        """
        # 初始化粒子位置
        particles = np.zeros((self.params.num_particles, 5))
        
        # 方位角: [0, 2π)
        particles[:, 0] = np.random.uniform(self.params.min_theta, self.params.max_theta, self.params.num_particles)
        
        # 仰角: [-π/6, π/6]
        particles[:, 1] = np.random.uniform(self.params.min_phi, self.params.max_phi, self.params.num_particles)
        
        # 无人机速度: [70, 140]
        particles[:, 2] = np.random.uniform(self.params.min_uav_speed, self.params.max_uav_speed, self.params.num_particles)
        
        # 投放时间: [0, 46.58]
        particles[:, 3] = np.random.uniform(0, self.params.max_drop_time, self.params.num_particles)
        
        # 起爆延迟: [0.5, 10] (上限为经验值)
        particles[:, 4] = np.random.uniform(self.params.min_delay, 10.0, self.params.num_particles)
        
        # 初始化粒子速度
        max_velocities = np.array([
            0.1 * (self.params.max_theta - self.params.min_theta),  # 方位角速度范围
            0.1 * (self.params.max_phi - self.params.min_phi),      # 仰角速度范围
            0.1 * (self.params.max_uav_speed - self.params.min_uav_speed),  # 速度变化范围
            0.1 * self.params.max_drop_time,  # 投放时间变化范围
            0.1 * 10.0  # 起爆延迟变化范围
        ])
        
        velocities = np.random.uniform(-max_velocities, max_velocities, (self.params.num_particles, 5))
        
        return particles, velocities
    
    def _check_convergence(self) -> bool:
        """检查算法是否收敛
        
        Returns:
            若收敛则返回True，否则返回False
        """
        if len(self.global_best_history) < self.params.convergence_window:
            return False
            
        # 计算最近window次迭代的最优值变化率
        recent_values = self.global_best_history[-self.params.convergence_window:]
        max_val = max(recent_values)
        min_val = min(recent_values)
        
        if max_val < 1e-6:
            return True
            
        return (max_val - min_val) / max_val < self.params.convergence_threshold
    
    def optimize(self) -> Dict:
        """运行优化算法
        
        Returns:
            包含优化结果的字典
        """
        logger.info("开始优化烟幕干扰弹投放策略...")
        start_time = time.time()
        
        # 初始化粒子群
        particles, velocities = self._initialize_particles()
        
        # 计算初始适应度
        fitness = np.array([self._calculate_fitness(p) for p in particles])
        
        # 初始化个体最优和全局最优
        personal_best = np.copy(particles)
        personal_best_fitness = np.copy(fitness)
        
        global_best_idx = np.argmax(personal_best_fitness)
        global_best = np.copy(personal_best[global_best_idx])
        global_best_fitness = personal_best_fitness[global_best_idx]
        
        # 记录历史
        self.fitness_history.append(np.mean(fitness))
        self.global_best_history.append(global_best_fitness)
        
        logger.info(f"初始最优适应度: {global_best_fitness:.4f}")
        
        # 开始迭代
        for iter in range(self.params.max_iterations):
            # 计算自适应惯性权重
            w = self.params.w_max - (self.params.w_max - self.params.w_min) * (iter / self.params.max_iterations)
            
            # 更新粒子速度和位置
            for i in range(self.params.num_particles):
                # 计算认知和社会成分
                cognitive = self.params.c1 * np.random.rand() * (personal_best[i] - particles[i])
                social = self.params.c2 * np.random.rand() * (global_best - particles[i])
                
                # 更新速度
                velocities[i] = w * velocities[i] + cognitive + social
                
                # 更新位置
                particles[i] += velocities[i]
                
                # 边界处理
                particles[i, 0] = np.clip(particles[i, 0], self.params.min_theta, self.params.max_theta)
                particles[i, 1] = np.clip(particles[i, 1], self.params.min_phi, self.params.max_phi)
                particles[i, 2] = np.clip(particles[i, 2], self.params.min_uav_speed, self.params.max_uav_speed)
                particles[i, 3] = np.clip(particles[i, 3], 0, self.params.max_drop_time)
                particles[i, 4] = np.clip(particles[i, 4], self.params.min_delay, 10.0)
            
            # 计算新的适应度
            fitness = np.array([self._calculate_fitness(p) for p in particles])
            
            # 更新个体最优
            improved = fitness > personal_best_fitness
            personal_best[improved] = np.copy(particles[improved])
            personal_best_fitness[improved] = fitness[improved]
            
            # 更新全局最优
            current_best_idx = np.argmax(personal_best_fitness)
            current_best_fitness = personal_best_fitness[current_best_idx]
            
            if current_best_fitness > global_best_fitness:
                global_best = np.copy(personal_best[current_best_idx])
                global_best_fitness = current_best_fitness
            
            # 精英学习策略：定期对全局最优进行扰动
            if (iter + 1) % self.params.perturbation_interval == 0:
                # 生成扰动
                perturbation = self.params.perturbation_strength * np.array([
                    (self.params.max_theta - self.params.min_theta) * np.random.randn(),
                    (self.params.max_phi - self.params.min_phi) * np.random.randn(),
                    (self.params.max_uav_speed - self.params.min_uav_speed) * np.random.randn(),
                    self.params.max_drop_time * np.random.randn(),
                    10.0 * np.random.randn()  # 起爆延迟的扰动范围
                ])
                
                # 应用扰动
                perturbed_best = global_best + perturbation
                
                # 边界处理
                perturbed_best[0] = np.clip(perturbed_best[0], self.params.min_theta, self.params.max_theta)
                perturbed_best[1] = np.clip(perturbed_best[1], self.params.min_phi, self.params.max_phi)
                perturbed_best[2] = np.clip(perturbed_best[2], self.params.min_uav_speed, self.params.max_uav_speed)
                perturbed_best[3] = np.clip(perturbed_best[3], 0, self.params.max_drop_time)
                perturbed_best[4] = np.clip(perturbed_best[4], self.params.min_delay, 10.0)
                
                # 计算扰动后的适应度
                perturbed_fitness = self._calculate_fitness(perturbed_best)
                
                # 如果更优则更新
                if perturbed_fitness > global_best_fitness:
                    global_best = perturbed_best
                    global_best_fitness = perturbed_fitness
                    logger.info(f"迭代 {iter+1}: 全局最优被扰动改进，新适应度: {global_best_fitness:.4f}")
            
            # 记录历史
            self.fitness_history.append(np.mean(fitness))
            self.global_best_history.append(global_best_fitness)
            
            # 打印进度
            if (iter + 1) % 10 == 0:
                logger.info(f"迭代 {iter+1}/{self.params.max_iterations}: 平均适应度 = {np.mean(fitness):.4f}, 全局最优 = {global_best_fitness:.4f}")
            
            # 检查收敛
            if self._check_convergence():
                logger.info(f"算法在迭代 {iter+1} 收敛")
                break
        
        # 保存最优解
        self.best_solution = global_best
        self.best_fitness = global_best_fitness
        
        # 计算优化耗时
        end_time = time.time()
        logger.info(f"优化完成，耗时 {end_time - start_time:.2f} 秒")
        logger.info(f"最优遮蔽时长: {self.best_fitness:.4f} 秒")
        
        # 解析最优解
        theta, phi, v_u, t_t, delta_tq = self.best_solution
        t_q = t_t + delta_tq
        release_pos = self._get_smoke_release_position(theta, phi, v_u, t_t)
        detonate_pos = self._get_smoke_detonate_position(theta, phi, v_u, t_t, delta_tq)
        
        return {
            'best_solution': self.best_solution,
            'best_fitness': self.best_fitness,
            'theta': theta,
            'phi': phi,
            'v_u': v_u,
            't_t': t_t,
            'delta_tq': delta_tq,
            't_q': t_q,
            'release_position': release_pos,
            'detonate_position': detonate_pos,
            'fitness_history': self.fitness_history,
            'global_best_history': self.global_best_history,
            'execution_time': end_time - start_time
        }
    
    def plot_convergence(self, figsize: Tuple[int, int] = (10, 6)) -> None:
        """绘制收敛曲线
        
        Args:
            figsize: 图表尺寸
        """
        if not self.fitness_history:
            logger.warning("没有优化历史数据，请先运行优化")
            return
            
        plt.figure(figsize=figsize)
        plt.plot(range(len(self.fitness_history)), self.fitness_history, label='平均适应度')
        plt.plot(range(len(self.global_best_history)), self.global_best_history, label='全局最优适应度')
        plt.xlabel('迭代次数')
        plt.ylabel('适应度 (遮蔽时长，秒)')
        plt.title('算法收敛曲线')
        plt.legend()
        plt.grid(True)
        plt.show()
    
    def plot_3d_trajectory(self, figsize: Tuple[int, int] = (12, 10)) -> None:
        """绘制3D轨迹图
        
        Args:
            figsize: 图表尺寸
        """
        if not self.best_solution:
            logger.warning("没有最优解数据，请先运行优化")
            return
            
        # 解析最优解
        theta, phi, v_u, t_t, delta_tq = self.best_solution
        t_q = t_t + delta_tq
        
        # 计算关键位置
        uav_start_pos = self.params.uav_initial_pos
        release_pos = self._get_smoke_release_position(theta, phi, v_u, t_t)
        detonate_pos = self._get_smoke_detonate_position(theta, phi, v_u, t_t, delta_tq)
        
        # 计算无人机轨迹
        uav_times = np.linspace(0, t_t, 100)
        uav_trajectory = [self._get_uav_position(t, theta, phi, v_u) for t in uav_times]
        uav_x, uav_y, uav_z = zip(*uav_trajectory)
        
        # 计算导弹轨迹
        missile_times = np.linspace(0, self.missile_hit_time, 200)
        missile_trajectory = [self._get_missile_position(t) for t in missile_times]
        missile_x, missile_y, missile_z = zip(*missile_trajectory)
        
        # 计算烟幕轨迹
        smoke_times = np.linspace(t_q, t_q + self.params.smoke_duration, 100)
        smoke_trajectory = [self._get_smoke_position(detonate_pos, t, t_q) for t in smoke_times]
        smoke_x, smoke_y, smoke_z = zip(*smoke_trajectory)
        
        # 创建3D图
        fig = plt.figure(figsize=figsize)
        ax = fig.add_subplot(111, projection='3d')
        
        # 绘制无人机轨迹
        ax.plot(uav_x, uav_y, uav_z, 'b-', label='无人机轨迹')
        ax.scatter([uav_start_pos[0]], [uav_start_pos[1]], [uav_start_pos[2]], c='blue', s=100, label='无人机起点')
        ax.scatter([release_pos[0]], [release_pos[1]], [release_pos[2]], c='cyan', s=100, label='烟幕投放点')
        
        # 绘制导弹轨迹
        ax.plot(missile_x, missile_y, missile_z, 'r-', label='导弹轨迹')
        ax.scatter([self.params.missile_initial_pos[0]], [self.params.missile_initial_pos[1]], 
                  [self.params.missile_initial_pos[2]], c='red', s=100, label='导弹起点')
        
        # 绘制烟幕轨迹
        ax.plot(smoke_x, smoke_y, smoke_z, 'g-', label='烟幕轨迹')
        ax.scatter([detonate_pos[0]], [detonate_pos[1]], [detonate_pos[2]], c='green', s=100, label='烟幕起爆点')
        
        # 绘制真目标
        target_x, target_y, target_z = self.params.target_center
        u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
        sphere_x = target_x + self.params.target_radius * np.cos(u) * np.sin(v)
        sphere_y = target_y + self.params.target_radius * np.sin(u) * np.sin(v)
        sphere_z = target_z + self.params.target_radius * np.cos(v)
        ax.plot_wireframe(sphere_x, sphere_y, sphere_z, color="purple", alpha=0.3)
        
        # 绘制真目标高度
        ax.plot([target_x, target_x], [target_y, target_y], 
                [target_z, target_z + self.params.target_height], 'p-', alpha=0.3)
        
        ax.set_xlabel('X (米)')
        ax.set_ylabel('Y (米)')
        ax.set_zlabel('Z (米)')
        ax.set_title('烟幕干扰弹投放策略3D轨迹图')
        ax.legend()
        plt.show()
    
    def animate_simulation(self, duration: float = 10.0) -> None:
        """生成仿真动画
        
        Args:
            duration: 动画持续时间(秒)
        """
        if not self.best_solution:
            logger.warning("没有最优解数据，请先运行优化")
            return
            
        # 解析最优解
        theta, phi, v_u, t_t, delta_tq = self.best_solution
        t_q = t_t + delta_tq
        
        # 计算关键位置
        release_pos = self._get_smoke_release_position(theta, phi, v_u, t_t)
        detonate_pos = self._get_smoke_detonate_position(theta, phi, v_u, t_t, delta_tq)
        
        # 确定动画时间范围
        start_time = max(0, t_q - 5)  # 起爆前5秒开始
        end_time = min(self.missile_hit_time, t_q + self.params.smoke_duration + 5)  # 烟幕消失后5秒结束
        total_time = end_time - start_time
        frame_count = int(30 * duration)  # 30帧/秒
        times = np.linspace(start_time, end_time, frame_count)
        
        # 创建图形
        fig = plt.figure(figsize=(12, 10))
        ax = fig.add_subplot(111, projection='3d')
        
        # 初始化绘图元素
        uav_line, = ax.plot([], [], [], 'b-', label='无人机轨迹')
        uav_marker, = ax.plot([], [], [], 'bo', markersize=8)
        missile_line, = ax.plot([], [], [], 'r-', label='导弹轨迹')
        missile_marker, = ax.plot([], [], [], 'ro', markersize=8)
        smoke_marker, = ax.plot([], [], [], 'go', markersize=10)
        smoke_sphere = ax.plot_wireframe([], [], [], color="green", alpha=0.3)
        
        # 绘制真目标
        target_x, target_y, target_z = self.params.target_center
        u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
        sphere_x = target_x + self.params.target_radius * np.cos(u) * np.sin(v)
        sphere_y = target_y + self.params.target_radius * np.sin(u) * np.sin(v)
        sphere_z = target_z + self.params.target_radius * np.cos(v)
        ax.plot_wireframe(sphere_x, sphere_y, sphere_z, color="purple", alpha=0.3)
        ax.plot([target_x, target_x], [target_y, target_y], 
                [target_z, target_z + self.params.target_height], 'p-', alpha=0.3)
        
        # 设置坐标轴范围
        all_x = [self.params.uav_initial_pos[0], self.params.missile_initial_pos[0], 
                release_pos[0], detonate_pos[0], target_x]
        all_y = [self.params.uav_initial_pos[1], self.params.missile_initial_pos[1], 
                release_pos[1], detonate_pos[1], target_y]
        all_z = [self.params.uav_initial_pos[2], self.params.missile_initial_pos[2], 
                release_pos[2], detonate_pos[2], target_z, target_z + self.params.target_height]
        
        x_range = max(all_x) - min(all_x)
        y_range = max(all_y) - min(all_y)
        z_range = max(all_z) - min(all_z)
        max_range = max(x_range, y_range, z_range)
        
        ax.set_xlim(min(all_x) - 0.1*max_range, max(all_x) + 0.1*max_range)
        ax.set_ylim(min(all_y) - 0.1*max_range, max(all_y) + 0.1*max_range)
        ax.set_zlim(min(all_z) - 0.1*max_range, max(all_z) + 0.1*max_range)
        
        ax.set_xlabel('X (米)')
        ax.set_ylabel('Y (米)')
        ax.set_zlabel('Z (米)')
        ax.set_title('烟幕干扰弹投放仿真')
        ax.legend()
        
        # 初始化函数
        def init():
            uav_line.set_data([], [])
            uav_line.set_3d_properties([])
            uav_marker.set_data([], [])
            uav_marker.set_3d_properties([])
            missile_line.set_data([], [])
            missile_line.set_3d_properties([])
            missile_marker.set_data([], [])
            missile_marker.set_3d_properties([])
            smoke_marker.set_data([], [])
            smoke_marker.set_3d_properties([])
            return uav_line, uav_marker, missile_line, missile_marker, smoke_marker, smoke_sphere
        
        # 更新函数
        def update(frame):
            try:
                t = times[frame]
                
                # 更新无人机
                if t <= t_t:  # 无人机在投放前
                    try:
                        uav_pos = self._get_uav_position(t, theta, phi, v_u)
                        # 绘制从起点到当前位置的轨迹
                        prev_times = np.linspace(0, t, 50)
                        prev_positions = [self._get_uav_position(tt, theta, phi, v_u) for tt in prev_times]
                        px, py, pz = zip(*prev_positions)
                        uav_line.set_data(px, py)
                        uav_line.set_3d_properties(pz)
                        uav_marker.set_data([uav_pos[0]], [uav_pos[1]])
                        uav_marker.set_3d_properties([uav_pos[2]])
                    except Exception as e:
                        logger.warning(f"更新无人机位置时出错: {e}")
                        uav_marker.set_data([], [])
                        uav_marker.set_3d_properties([])
                else:  # 投放后不再显示无人机
                    uav_marker.set_data([], [])
                    uav_marker.set_3d_properties([])
                
                # 更新导弹
                try:
                    missile_pos = self._get_missile_position(t)
                    # 绘制从起点到当前位置的轨迹
                    prev_times = np.linspace(0, t, 50)
                    prev_positions = [self._get_missile_position(tt) for tt in prev_times]
                    mx, my, mz = zip(*prev_positions)
                    missile_line.set_data(mx, my)
                    missile_line.set_3d_properties(mz)
                    missile_marker.set_data([missile_pos[0]], [missile_pos[1]])
                    missile_marker.set_3d_properties([missile_pos[2]])
                except Exception as e:
                    logger.warning(f"更新导弹位置时出错: {e}")
                    missile_marker.set_data([], [])
                    missile_marker.set_3d_properties([])
                
                # 更新烟幕
                if t >= t_q and t <= t_q + self.params.smoke_duration:
                    try:
                        smoke_pos = self._get_smoke_position(detonate_pos, t, t_q)
                        smoke_marker.set_data([smoke_pos[0]], [smoke_pos[1]])
                        smoke_marker.set_3d_properties([smoke_pos[2]])
                        
                        # 更新烟幕球体
                        nonlocal smoke_sphere
                        try:
                            smoke_sphere.remove()
                        except:
                            pass  # 忽略第一次尝试删除不存在的球体
                        
                        sx, sy, sz = smoke_pos
                        smoke_x = sx + self.params.smoke_radius * np.cos(u) * np.sin(v)
                        smoke_y = sy + self.params.smoke_radius * np.sin(u) * np.sin(v)
                        smoke_z = sz + self.params.smoke_radius * np.cos(v)
                        smoke_sphere = ax.plot_wireframe(smoke_x, smoke_y, smoke_z, color="green", alpha=0.3)
                    except Exception as e:
                        logger.warning(f"更新烟幕时出错: {e}")
                        smoke_marker.set_data([], [])
                        smoke_marker.set_3d_properties([])
                else:
                    smoke_marker.set_data([], [])
                    smoke_marker.set_3d_properties([])
                
                return uav_line, uav_marker, missile_line, missile_marker, smoke_marker, smoke_sphere
            except IndexError:
                logger.warning(f"动画帧索引越界: {frame}")
                # 返回空数据以避免崩溃
                uav_marker.set_data([], [])
                uav_marker.set_3d_properties([])
                missile_marker.set_data([], [])
                missile_marker.set_3d_properties([])
                smoke_marker.set_data([], [])
                smoke_marker.set_3d_properties([])
                return uav_line, uav_marker, missile_line, missile_marker, smoke_marker, smoke_sphere
            except Exception as e:
                logger.error(f"动画更新时发生错误: {e}")
                # 返回空数据以避免崩溃
                uav_marker.set_data([], [])
                uav_marker.set_3d_properties([])
                missile_marker.set_data([], [])
                missile_marker.set_3d_properties([])
                smoke_marker.set_data([], [])
                smoke_marker.set_3d_properties([])
                return uav_line, uav_marker, missile_line, missile_marker, smoke_marker, smoke_sphere
        
        # 创建动画
        anim = animation.FuncAnimation(
            fig, update, frames=frame_count, init_func=init,
            interval=duration*1000/frame_count, blit=True
        )
        
        plt.show()
        
        return anim


def parameter_tuning(param_ranges: Dict, num_trials: int = 5) -> Tuple[Parameters, float]:
    """参数调优函数
    
    Args:
        param_ranges: 参数范围字典
        num_trials: 每个参数组合的试验次数
        
    Returns:
        最佳参数配置和对应的平均适应度
    """
    logger.info("开始参数调优...")
    
    best_avg_fitness = -np.inf
    best_params = None
    
    # 生成所有参数组合（简化版，实际可使用更高效的调优方法）
    param_names = list(param_ranges.keys())
    param_values = [param_ranges[name] for name in param_names]
    
    # 简单网格搜索（实际应用中可使用更高效的方法）
    try:
        from itertools import product
    except ImportError:
        logger.error("导入itertools.product失败")
        return Parameters(), -np.inf
    
    for params_tuple in product(*param_values):
        # 创建参数对象
        param_dict = dict(zip(param_names, params_tuple))
        current_params = Parameters(**param_dict)
        
        # 多次运行取平均值
        total_fitness = 0.0
        
        for trial in range(num_trials):
            logger.info(f"测试参数组合 {param_dict}, 试验 {trial+1}/{num_trials}")
            optimizer = SmokeOptimizer(current_params)
            result = optimizer.optimize()
            total_fitness += result['best_fitness']
        
        avg_fitness = total_fitness / num_trials
        logger.info(f"参数组合 {param_dict} 的平均适应度: {avg_fitness:.4f}")
        
        # 更新最佳参数
        if avg_fitness > best_avg_fitness:
            best_avg_fitness = avg_fitness
            best_params = current_params
            logger.info(f"找到更优参数组合，平均适应度: {avg_fitness:.4f}")
    
    logger.info("参数调优完成")
    return best_params, best_avg_fitness


def performance_test(params: Optional[Parameters] = None, num_runs: int = 5) -> Dict:
    """性能测试函数
    
    Args:
        params: 参数配置
        num_runs: 测试次数
        
    Returns:
        包含性能统计信息的字典
    """
    logger.info("开始性能测试...")
    
    results = {
        'fitness': [],
        'time': [],
        'convergence_iter': []
    }
    
    for run in range(num_runs):
        logger.info(f"性能测试运行 {run+1}/{num_runs}")
        optimizer = SmokeOptimizer(params)
        start_time = time.time()
        result = optimizer.optimize()
        end_time = time.time()
        
        results['fitness'].append(result['best_fitness'])
        results['time'].append(end_time - start_time)
        results['convergence_iter'].append(len(optimizer.global_best_history))
    
    # 计算统计信息
    stats = {
        'avg_fitness': np.mean(results['fitness']),
        'std_fitness': np.std(results['fitness']),
        'min_fitness': np.min(results['fitness']),
        'max_fitness': np.max(results['fitness']),
        'avg_time': np.mean(results['time']),
        'std_time': np.std(results['time']),
        'avg_convergence_iter': np.mean(results['convergence_iter']),
    }
    
    logger.info("性能测试结果:")
    logger.info(f"平均遮蔽时长: {stats['avg_fitness']:.4f} ± {stats['std_fitness']:.4f} 秒")
    logger.info(f"平均优化时间: {stats['avg_time']:.4f} ± {stats['std_time']:.4f} 秒")
    logger.info(f"平均收敛迭代次数: {stats['avg_convergence_iter']:.2f}")
    
    return stats


def main():
    """主函数：演示使用示例"""
    # 添加命令行参数解析
    import argparse
    parser = argparse.ArgumentParser(description='烟幕干扰弹最优投放策略PSO优化')
    parser.add_argument('--num-particles', type=int, default=80, help='粒子数量')
    parser.add_argument('--max-iterations', type=int, default=200, help='最大迭代次数')
    parser.add_argument('--w-max', type=float, default=0.9, help='惯性权重最大值')
    parser.add_argument('--w-min', type=float, default=0.4, help='惯性权重最小值')
    parser.add_argument('--c1', type=float, default=2.0, help='认知加速度系数')
    parser.add_argument('--c2', type=float, default=2.0, help='社会加速度系数')
    parser.add_argument('--show-animation', action='store_true', help='显示动画模拟')
    parser.add_argument('--run-performance-test', action='store_true', help='运行性能测试')
    parser.add_argument('--performance-runs', type=int, default=3, help='性能测试运行次数')
    parser.add_argument('--animation-duration', type=int, default=10, help='动画持续时间（秒）')
    args = parser.parse_args()
    
    try:
        # 创建参数配置
        params = Parameters(
            num_particles=args.num_particles,
            max_iterations=args.max_iterations,
            w_max=args.w_max,
            w_min=args.w_min,
            c1=args.c1,
            c2=args.c2
        )
        
        logger.info(f"使用参数配置: {vars(params)}")
        
        # 创建优化器并运行优化
        optimizer = SmokeOptimizer(params)
        result = optimizer.optimize()
        
        # 打印最优解
        print("\n===== 最优投放策略 =====")
        print(f"最佳遮蔽时长: {result['best_fitness']:.4f} 秒")
        print(f"无人机方位角: {np.rad2deg(result['theta']):.2f} 度")
        print(f"无人机仰角: {np.rad2deg(result['phi']):.2f} 度")
        print(f"无人机速度: {result['v_u']:.2f} 米/秒")
        print(f"投放时间: {result['t_t']:.2f} 秒")
        print(f"起爆延迟: {result['delta_tq']:.2f} 秒")
        print(f"起爆时间: {result['t_q']:.2f} 秒")
        print(f"投放点位置: ({result['release_position'][0]:.2f}, {result['release_position'][1]:.2f}, {result['release_position'][2]:.2f}) 米")
        print(f"起爆点位置: ({result['detonate_position'][0]:.2f}, {result['detonate_position'][1]:.2f}, {result['detonate_position'][2]:.2f}) 米")
        
        # 绘制收敛曲线
        optimizer.plot_convergence()
        
        # 绘制3D轨迹
        optimizer.plot_3d_trajectory()
        
        # 生成动画
        if args.show_animation:
            try:
                anim = optimizer.animate_simulation(duration=args.animation_duration)
            except Exception as e:
                logger.error(f"生成动画时出错: {e}")
        
        # 性能测试
        if args.run_performance_test:
            try:
                performance_stats = performance_test(params, num_runs=args.performance_runs)
            except Exception as e:
                logger.error(f"运行性能测试时出错: {e}")
        
    except ValueError as ve:
        logger.error(f"参数值错误: {ve}")
    except TypeError as te:
        logger.error(f"参数类型错误: {te}")
    except MemoryError:
        logger.error("内存不足，请减少粒子数量或迭代次数")
    except Exception as e:
        logger.error(f"程序执行过程中出错: {e}", exc_info=True)


if __name__ == "__main__":
    try:
        main()
    except Exception as e:
        logger.error(f"程序运行出错: {str(e)}", exc_info=True)
